#include "timer.hpp"
//#include "devices.hpp"




void Timer_Init(void)
{
	pit_init();
	pit_interrupt_ms(PIT_CH0,1);
	pit_interrupt_ms(PIT_CH1,10);
	NVIC_SetPriority(PIT_IRQn,4);
}



short int cnt100ms;
// short int magcnt=0,magtime=-1,releas_sort=0;

unsigned char buzzflag=0,clock_startflag=0;

short int clock_s=0,clock_ms=0;

void Timer_1ms_Callback (void)
{
	//计时
	if(clock_startflag)
	{
		if(clock_ms>999)
		{
			clock_ms=0;
			clock_s++;
		}
		clock_ms++;
	}
	//陀螺仪数据处理
	get_icm20602_data_spi();
	if(Attitude_Offsetflag==1)
	{
		icm_fgyro_z=IIRLowPass(icm_cgyro_z, groy_filter);
		if(icm_fgyro_z>3||icm_fgyro_z<-3)
		yaw-=icm_fgyro_z/16.4*0.001f;
	}
	//Buzz
  	if(cnt100ms==50)
	{
		cnt100ms=0;
		if(buzzflag!=0)
		{
			buzzflag--;
			gpio_toggle(D26);
		}	
	}
  cnt100ms++;
}

void Timer_10ms_Callback (void)
{
	
	if(Attitude_Offsetflag==1)
	{
		LGH_Motor1.Encoder_Delta = -Encoder1.get();
		LGH_Motor2.Encoder_Delta = Encoder2.get();
		LGH_Motor3.Encoder_Delta = Encoder3.get();
		LGH_Motor4.Encoder_Delta = -Encoder4.get();
		

		Encoder1.reset();
		Encoder2.reset();
		Encoder3.reset();
		Encoder4.reset();

		LGH_Motor1.Encoder_Sum+=LGH_Motor1.Encoder_Delta;
		LGH_Motor2.Encoder_Sum+=LGH_Motor2.Encoder_Delta;
		LGH_Motor3.Encoder_Sum+=LGH_Motor3.Encoder_Delta;
		LGH_Motor4.Encoder_Sum+=LGH_Motor4.Encoder_Delta;
				
		Car_Encoder_SumX+=LGH_Motor1.Encoder_Delta
										+LGH_Motor2.Encoder_Delta
										+LGH_Motor3.Encoder_Delta
										+LGH_Motor4.Encoder_Delta;
		
//		Car_Encoder_SumY+=LGH_Motor1.Encoder_Delta
//										 -LGH_Motor2.Encoder_Delta
//										 +LGH_Motor3.Encoder_Delta
//										 -LGH_Motor4.Encoder_Delta;
//			
//		Car_Encoder_SumZ+=LGH_Motor1.Encoder_Delta
//										 -LGH_Motor2.Encoder_Delta
//										 -LGH_Motor3.Encoder_Delta
//										 +LGH_Motor4.Encoder_Delta;

		Set_Motor_Pwm();
	}

}